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Abstract — This paper introduces a global uncertainty propa- 
gation scheme for rigid body dynamics, through a combination 
of numerical parametric uncertainty techniques, noncommu- 
tative harmonic analysis, and geometric numerical integration. 
This method is distinguished from prior approaches, as it allows 
one to consider probability densities that are global, and are not 
supported on only a single coordinate chart on the manifold. 
The use of Lie group variational integrators, that are symplectic 
and stay on the Lie group, as the underlying numerical 
propagator ensures that the advected probability densities 
respect the geometric properties of uncertainty propagation 
in Hamiltonian systems, which arise as consequence of the 
Gromov nonsqueezing theorem from symplectic geometry. We 
also describe how the global uncertainty propagation scheme 
can be applied to the problem of global attitude estimation. 

I. Introduction 

A nonlinear uncertainty propagation scheme is developed 
for the dynamics of a rigid body, viewed as evolving on 
the special orthogonal group SO (3). Rigid body dynamics 
is a Hamiltonian flow on a Lie group, and most current 
attitude uncertainty propagation schemes [1] do not properly 
take these characteristics into account. Typically, the attitude 
is represented by unit quaternions, which is problematic 
for global uncertainty propagation due to the ambiguity 
introduced by the double cover of SO (3) by the three- 
sphere S 3 of unit quaternions. Furthermore, the dynamics 
are often simplified to kinematic equations, thereby ignoring 
uncertainties in the angular velocity. As such, most existing 
techniques are only valid over time periods when the uncer- 
tainties are small. 

The method introduced in this paper is focused on devel- 
oping a global uncertainty propagation method for a rigid 
body by explicitly considering the characteristics of the 
Hamiltonian flow and the configuration manifold, without 
implicitly assuming that the uncertainty is localized, nor that 
the uncertainty distribution is fully supported in a single 
coordinate chart on the manifold. 

Although it is not widely known in the engineering 
community, the theories of probability and stochastic pro- 
cesses on manifolds have been studied by theoretical statis- 
ticians [2], [3], [4]. Earlier works on attitude estimation on 
SO (3) include [5], where a probability density function is 
expressed using noncommutative harmonic analysis. This 
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idea of using fourier analysis on manifolds has been applied 
in [6], [7], [8], [9], and recently, results in [5] are extended 
to include the effects of process noise and sensor parameters 
in [10]. 

The Liouville equation describes the evolution of a prob- 
ability density function in the absence of external diffusion, 
and can be viewed as the deterministic analogue of the 
Fokker-Planck equation. When the flow that is advecting the 
probability density is Hamiltonian, the Liouville equation 
reduces to an ordinary differential equation [11]. Thus, a 
probability density function can be propagated using the 
flow map of the Hamiltonian system. In [12], an attitude 
estimation scheme is developed by linearizing the attitude 
dynamics along the mean obtained from the probability 
density function propagated by this property. However, non- 
linearities of the flow imply that numerical methods for 
propagating uncertainties using linearization rapidly degrade 
in performance, unless frequent physical measurements are 
available [13]. It is therefore desirable to construct efficient 
numerical methods for solving the Liouville equation that 
describes the evolution of a probability density advected by 
a prescribed Hamiltonian flow. 

In this paper, a global attitude uncertainty propagation 
scheme is developed in the absence of process noise. This 
is achieved through a synthesis of numerical parametric 
uncertainty analysis techniques [14], [15], noncommutative 
harmonic analysis [16], and geometric numerical integra- 
tion [17]. We backpropagate sample points along the Hamil- 
tonian flow of the rigid body dynamics, and we use the 
advected probabilities to reconstruct the probability density 
using noncommutative harmonic analysis on SO (3). This 
is in contrast to Monte Carlo methods, where the sample 
trajectories are used to compute statistical properties of the 
advected density. 

The Gromov nonsqueezing theorem [18] from symplectic 
geometry places fundamental limits on how the uncertainty 
of a Hamiltonian system evolves [19]. It is therefore essential 
that symplectic methods be used to propagate individual tra- 
jectories when analyzing uncertainty propagation in Hamil- 
tonian systems. We use a geometric numerical integrator, 
referred to as a Lie group variational integrator, that preserves 
the symplectic property of the Hamiltonian dynamics and the 
group structure of the configuration space [20], [21]. The 
purpose of this paper is to develop a computational method 
to propagate uncertainties under a Hamiltonian flow on a 
Lie group. Our development is for a specific Hamiltonian 
system, namely the 3D pendulum, that evolves on the Lie 
group SO (3). We also comment on how this development is 
applicable to attitude estimation. The methodology proposed 



in this paper should be distinguished from the numerical 
methods that compute a specific realization of a stochastic 
Hamiltonian system [22], [23], [24]. 

II. Attitude Dynamics of a Rigid Body 

A. 3D Pendulum 

The 3D pendulum is nontrivial example of a Hamiltonian 
system that evolves on a Lie group; this example is used in 
the subsequent development. A rigid 3D pendulum is a rigid 
body supported by a fixed, frictionless pivot, acted on by 
uniform gravitational force [25]. The supporting pivot allows 
the pendulum three rotational degrees of freedom. 

Two reference frames are introduced. An inertial reference 
frame has its origin at the pivot; the first two axes lie in the 
horizontal plane and the third axis is vertical in the direction 
of gravity. A reference frame fixed to the pendulum body is 
also introduced. The origin of this body-fixed frame is also 
located at the pivot. The configuration manifold is the special 
orthogonal group SO (3), 

SO(3) = {Re R 3x3 | R T R = J 3x3 , det R = 1} , 

where the rotation matrix R G SO (3) represents the linear 
transformation from the body-fixed frame to the inertial 
frame. 

The dynamics of the 3D pendulum are given by the Euler 
rigid body equation that includes the moment due to gravity: 

J ft = Jft x ft + mgp x R T e 3 , (1) 

where the angular velocity in the body-fixed frame is denoted 
by ft G R 3 , the inertia matrix is denoted by J G R 3x3 , and 
the vector p G R 3 represents the location of the center of 
mass in the body-fixed frame. The constants m and g denote 
the mass of the pendulum and the gravitational acceleration, 
respectively. The kinematic equation is 

R = RS(fL). (2) 

For a given vector x G R 3 , the 3x3 skew- symmetric matrix 
S(x) is defined so that S(x)y — x x y for all y G R 3 . 

There are two disjoint equilibria when the direction of 
gravity in the body fixed frame is collinear with the vector 
p\ the hanging equilibrium when R T es = pj ||p||, and 
the inverted equilibrium when R T es = —p/\\p\\. The 3D 
pendulum exhibits surprisingly rich and complicated attitude 
dynamics [13], and is therefore particularly appropriate for 
demonstrating the properties of our global attitude uncer- 
tainty propagation scheme. 

B. Lie Group Variational Integrator 

Lie group variational integrators preserve the group struc- 
ture without the use of local charts, reprojection, or con- 
straints, they are symplectic and momentum preserving, and 
they exhibit good energy behavior for an exponentially long 
time period. The following Lie group variational integrator 



for the attitude dynamics of a rigid body was presented 
in [20], [21]: 

hS(Jfl k + ^M k ) = F k J d - J d Fl, (3) 
R k+1 = R k F k , (4) 
Jfl k+1 = F£jn k + ^FlM k + ^M fe+ i, (5) 

where J d G R 3 x 3 is a nonstandard moment of inertia matrix 
given by J d = §tr[J] 7 3 x3 - J, and F k G SO(3) is the 
relative attitude between integration steps. The moment due 
to the potential is denoted by M k = mgp x R k es. The 
constant h G R is the integration step size, and the subscript 
k denotes the k-th integration step. This integrator yields 
a map (R k ,fl k ) i— > (R k +i,fl k +i) by solving © to obtain 
Fk G SO(3) and substituting it into © and © to obtain 
Rk+i and fifc+i. We use these discrete-time equations of 
motion to propagate the attitude dynamics. 

III. Global Symplectic Uncertainty Propagation 
on SO(3) 

Suppose that a probability density function for the attitude 
and the angular velocity of a rigid body at a time is 
given as p k (R,fl) : SO(3) x R 3 — > R. In this section, we 
develop a computational method to propagate this density 
along the Hamiltonian attitude dynamics assuming that there 
is no process noise. We represent the propagated probability 
density function using noncommutative harmonic analysis. 
A method for visualizing the attitude uncertainty is also 
discussed. 

A. Symplectic Uncertainty Propagation for a Hamiltonian 
System 

In [11], it is shown that the probability density function 
is preserved along a Hamiltonian flow on Euclidean space. 
In this subsection, we generalize this result to a general 
Hamiltonian system evolving on a manifold. 

Consider a Hamiltonian system on a 2n-dimensional 
symplectic manifold (Q,w), where Q is a 2n-dimensional 
manifold and uj : TQ x TQ — > R is a nondegenerate 
symplectic two-form on Q [26]. The Liouville volume form 
p : (TQ) 2n — > R is defined as the n-fold wedge product 

(_i\n(n-l)/2 

of the symplectic two-form with itself, p = 1 — <— } u A 

• • • Auj (n times). In local coordinates, this corresponds to 
the usual notion of the volume element in Euclidean spaces. 
Let CxP be the Lie derivative of the volume form p along 
a vector field X : Q — > TQ. The divergence of a vector 
field X on Q is defined as CxP — div M (X) p. Thus, the 
divergence div /i (X) represents the rate of change of a unit 
volume along the vector field X. 

In [27], it is shown that the Fokker-Planck equation for a 
dynamic system on a manifold in the absence of diffusion 
terms can be written as, 

^+div /i (pX) = 0. (6) 



Note that this has the same structure as the Euler equation for 
the density of compressible fluids. The existence and unique- 
ness of the solution of the equations of motion provides a 
property analogous to mass conservation in fluid dynamics. 
Since div M (pX) = pdiv^ (X) + CxP, the time derivative 
of the probability density function is given by 



d dp 

dt p= m+ CxP 



-pdiv^ (X). 



(7) 



If the vector field X is the Hamiltonian vector field on 
the divergence vanishes, div M (X) = according 
to the Liouville theorem [26]. Therefore, the Fokker-Planck 
equation for a deterministic Hamiltonian system is repre- 
sented by the ordinary differential equation 



d 

dt P = °- 



(8) 



This states that the probability density function is pre- 
served along a Hamiltonian flow without stochastic diffusion 
effects. More precisely, (8]) implies that the propagated prob- 
ability density function at tk+i can be explicitly expressed 
as a composition of the backward flow map and the given 
probability density function at tk 



(9) 



where T h : SO(3) x R 3 SO(3) x R 3 represents the k 
step discrete flow of the attitude dynamics. We can apply 
this equation recursively to propagate the probability density 
function over any time interval. 

B. Noncommutative Harmonic Analysis on SO (3) 

Equation © provides a method to compute the probability 
density function at any time based on the probability density 
function at some prior time. As the flow is nonlinear, it is in- 
efficient to characterize the density using its moments, since 
the moment expansion may decay slowly. Here, we propose a 
computational scheme that represents the probability density 
function for the attitude dynamics using noncommutative 
harmonic analysis on SO (3). 

Noncommutative harmonic analysis is a generalization of 
Fourier analysis on Euclidean spaces to manifolds [27] . This 
is particularly useful since it provides a mathematical tool to 
approximate a probability density function based on samples 
of that function. More precisely, the propagated probability 
density function for the attitude dynamics of a rigid body 
can be expressed as 

p(R, O) = ^ / exp(j0 • O) ir[P\0)U\R)] d0 : 

(10) 

where the vector 6 G R 3 and non-negative integer I G 
N |J{0} are Fourier parameters, and the set of complex ma- 
trices {P l (0) G C (2Z+1)X(2Z+1) }^ is the Fourier spectrum 
of the density p(R, ft). We denote the l-th irreducible unitary 
representation matrix of SO(3) by U l (R) G C^ 2l ^ x ^ 2W \ 
A representation of a group is a homomorphism from the 
group to the set of invertible matrices, and by the Peter- Weyl 



theorem [28], the irreducible unitary representations form a 
complete orthonormal basis for the set of square-integrable 
functions on the group. The irreducible unitary representa- 
tions can be expressed in various ways. For example, it can 
be expressed in terms of the wigner-d functions using 3-1-3 
Euler angles a, /?, 7 as 

U^ n (R(a,(3,j)) = i ro -"e-^+"^„(cos/3) (11) 

for — I < m, n < I [29]. A few wigner-d functions are given 
by 



d° (cos (3) = 1 
d 1 {cos 13) = 



l+cos/3 
2 

sin (3 

V2 
1 — cos (3 



sin (3 

cos (3 

sin (3 
V2 



1— cos (3 
2 

sin (3 

l+cos/3 



Higher order wigner-d functions can be obtained using a 
recursive formula [27]. 

From the orthnormal property of the irreducible unitary 
representation, the Fourier spectrum is computed by 

P l (0)= [ [ p(R,Q)exp(-je'Q)U l (R- 1 )dndR. 

JSO(3) Jr 3 



(12) 



The volume element for the rotation matrix dR represents the 
Haar measure of SO (3); it can be written in terms of 3-1-3 
Euler angles a, /?, 7 as dR(a, (3, 7) = — ^ sin (3 dadftdj. 

Substituting ©, we can compute the Fourier spectrum of 
the propagated probability density function. The propagated 
distribution can be reconstructed by (fTOl) . This is a global 
particle-based method to construct the propagated probability 
density function on SO(3) x R 3 . 

C. Visualization of the Attitude Uncertainty 

Let pr : SO (3) — > R be a probability density function 
on SO (3). For example, it can be obtained by integrating 
(fTUb over R 3 , i.e. pr(R) = f R3 p(R,Q)dQ. We propose a 
method for visualizing probability densities on SO (3) using 
three copies of two- spheres. The rotation matrix represents a 
linear transformation from a body fixed frame to an inertial 
frame. Therefore, the i-th column of the rotation matrix Rei 
represent the direction of the i-th body fixed axis in the 
inertial frame. Since the vector Rei lies on the two-sphere 
S 2 , we can visualize uncertainties of Rei on a sphere either 
by color shading or by contour lines. Three copies of these 
spheres, one for each of the body fixed axes, can be used to 
visualize uncertainties on SO (3). 

We find the marginal probability density of pr for each 
column of the rotation matrix. For given r G S 2 , define a 
subset of SO (3) as 



Hi(r) = {Re SO(S)\ Rei = r} . 



(13) 



This is a subgroup of SO (3) diffeomorphic to S 1 , and it 
represents the set of rotation matrices whose i-th column 
is equal to the given direction r. This subgroup can be 
parameterized by G S 1 . More explicitly, we can find an 
element R°(r) of Hi(r) (for example, we have R°(r) = 





(a) Density 1 



(b) Density 2 




(c) Density 3 
Fig. 1. Attitude uncertainty visualization example 



(d) Sample attitudes for the den- 
sity 3 



ex p(iSr s(et x r)) if 6i x r ^ 0) - Then ' the sub s rou P 

Hi(r) is parameterized as 

Hi(r) = {R°(r)expS(O ei ) \0eS}. (14) 

The corresponding quotient space is the two-sphere, 
SO (3) /Hi(r) ~ S 2 . Using the properties of integration on a 
quotient space of a Lie group, we have 



1 



SO(3) 



res 2 



Pr(R) dR 



±- / PR (R^r)expS(eei)de) dr. (15) 



Therefore, the marginal probability density for the i-th col- 
umn of the rotation matrix, p l R : S 2 — > R is given by 



-/ 



^(^°(r)exp(^))^. (16) 



We plot these marginal probability density functions 
p l R (r), that represent the probability density of the direction 
of the body fixed axes, on three two- spheres. If the magnitude 
of uncertainties is small, we can plot uncertainties for each 
body fixed axis on a single sphere, from which we can 
intuitively understand the attitude uncertainty of the rigid 
body. Fig. [T] shows examples for the attitude probability 
density visualization. It is easily to see that the second 
density in Fig. |l(b)| has smaller variation than the first density 
in Fig. | l(b)[ Fig. |l(d)| shows some sample attitudes that we 
are likely to obtain from the third density given in Fig. 
|l(c)[ which reflect the fact that the direction of the z-axis 
of the body in the inertial frame is well localized, but there 
is greater uncertainty in the direction of the x- and y-axes. 

IV. Numerical Computations 

In this section, we propagate an initial probability density 
along the nontrivial dynamics of the 3D pendulum, and 



we visualize the attitude uncertainty. The properties of the 
pendulum are given by 

J = diag[0.13, 0.28, 0.17] kgm 2 , m = 1 kg, 
p = 0.3ea m, g = 9.81m/s 2 . 

The von Mises distribution (also known as the circular 
normal distribution) is a continuous probability density on 
the circle, which can be thought of as the circular analogue 
of the normal density [30]. 

where Io is the zeroth order modified Bessel function of the 
first kind, given by Iq(k) = ^^iT^ with parameters 

ft, 6 that determine the shape of the density; as n increases, 
the density approaches a normal density with mean and 
variance -. 

K 

For two given rotation matrices R,R G SO (3), the 
quantity ^(tr R R — 1) represents the cosine of the rotation 
angle between the two attitudes. Using this, we define a 
probability density function on SO (3) from the von Mises 
distribution. The probability distribution at the initial time is 
chosen as 



Po(R, Q) = - exp 
c 



x exp 



i«(tr 



— T 

Rq r 



o)}, 



(18) 



where R = hxs, = [4.14, 4.14, 4.14] rad/s, S = 
0.1414 2 /3 X 3, and n = 8. The constant c is a scaling factor 
chosen such that J p(R,Q) dRdfl = 1. The corresponding 
mean (i?o,^o) yields an irregular, perhaps chaotic, attitude 
response [13]. 

We propagate this initial distribution using © and com- 
pute the Fourier spectrum using (TT2b . The volume integration 
is approximated by Simpson's rule, and the flow map is 
computed using the Lie group variational integrator. The ap- 
plication of the Lie group variational integrator is particularly 
useful since it is symplectic, group structure preserving, and 
time reversible. 

We have developed a parallel computing code using the 
MPI (Message Passing Interface) library, where the domain 
of integration is divided uniformly and distributed to each 
processor. This is desirable in terms of minimizing commu- 
nication between processors and balancing the computational 
load among the processors. This algorithm has been imple- 
mented on 32 AMD Opeteron processors. Fig. [2] illustrates 
the propagated attitude uncertainty. 

V. Comments on Global Attitude Estimation 

The presented uncertainty propagation method can be 
applied to develop an attitude estimation scheme using Bayes 
rule. We first define a measurement model. We assume that 
the attitude and the angular velocity are measured by sensors 
to obtain 



z k = H(R k ,n k ) 



(19) 




(a) £ = 0.0 (Jo) t = 0.1 




(c) t = 0.2 (d) t = 0.4 




(e) t = 1.0 

Fig. 2. Propagated attitude uncertainty and max mean attitude 

where H : SO (3) x R 3 — > R m is a measurement function, 
Zk G M m is the measured value, and Vfc G M m is measure- 
ment noise. For example, if we measure a direction to a 
known object a G S 2 and the angular velocity, the measure- 
ment function can be written as H(R,fl) = [R T a;Q]. We 
assume the probability density of the measurement noise Vk 
is given by p Zk \k, and it is an independent process. 

The set of all measurements from the initial time to tk is 
denoted by the capital letter Zk = [zq, z\, . . . , Zk]. Suppose 
that we have a probability density function at the fc-th time 
conditioned by Zk, i.e. the expression for Pk\z k is known, 
and a new measurement is obtained Zk+i at tfc+i. Estimation 
can be described as finding an expression for Pk+i\z k + 1 given 
p k \z k and Z k +i = [Z k: z k +i]. Using Bayes rule [31], we 
have 

= Pz k+1 \k+i,z k (MR, ft, Z) p k+ i\z k (R, ®\Z) 
p Zk+1 \z k (z\Z) 

(20) 

Since the measurement processes are independent, we 
have p Zk ^\k + i,z k = Pz k+1 \k+i, and the propagated 
density is given by ©, i.e. p k+1 \ Zk (R,£l\Z) = 
Pk\z k (3 r ~ 1 (R,ft)\Z). The denominator is a normalizing 
constant that can be computed to satisfy p Zk+1 \z k ( z \%) ~ 
Jso(3)xRzPz k+1 \k+i(RM)Pk+i\z k (RM\Z)dRdn. In sum- 
mary, the propagated probability density conditioned by the 



new measurement is given by 

Pk+1 \ Zk+1 (R,n\[z,*]) 

= \pz k+1 \k+i(z\R,Q) p k \ Zk (F-\R^)\z) 

(21) 

for a constant c. This also can be represented using the 
harmonic analysis as in (fTOb . 

This expression is the key to solving the attitude estimation 
problem. This expression for p Xk+1 \z k+1 contains all the 
statistical information that can be derived from the attitude 
dynamics and the available measurements. 

VI. Conclusions 

This paper addresses the problem of propagating the 
attitude uncertainty of a rigid body which evolves on the 
rotation group. The use of noncommutative harmonic anal- 
ysis techniques to represent the uncertainty distribution in a 
global fashion overcomes a fundamental limitation of exist- 
ing techniques, which implicitly assume that the uncertainty 
is localized or small. By exploiting the fact that the Liouville 
equation for a Hamiltonian system reduces to an ordinary 
differential equation, we are able to adopt a particle based 
approach for computing the advected probability density, 
thereby avoiding the computational expense of solving the 
equation as a numerical partial differential equation. By 
adopting a Lie group variational integrator as the underlying 
numerical scheme, we ensure that the resulting uncertainty 
propagation method inherits the geometric properties of 
the time evolution of probability densities in Hamiltonian 
systems, that arise from the symplectic geometry of the phase 
space. 

A natural application of the proposed scheme is to the 
problem of global attitude estimation, particularly when the 
dynamics of the rigid body are extremely nonlinear, and the 
attitude measurements are relatively infrequent. Estimation 
problems with such characteristics are problematic for tradi- 
tional techniques, such as the Kalman filter, which require 
frequent measurements, and relatively benign dynamics, in 
order to justify the localization and linearization assumptions 
built into the method. 
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